#!/usr/bin/env python

import rospy
# from std_msgs.msg import String
from sensor_msgs.msg import JointState as Jstate
# import geometry_msgs.msg
# import time
import math
import os
import sys

pi = math.pi
path = os.path.abspath(".")
# 核心
sys.path.insert(0,path + "src/function_pkg/scripts")

from robotic_arm_pkg.robotic_arm import *


def m_callback(data):
    print("MCallbackMCallbackMCallback")
    # 判断接口类型
    if data.codeKey == MOVEJ_CANFD_CB:
        # 角度透传
        print("透传结果:", data.errCode)
        print("当前角度:", data.joint[0], data.joint[1], data.joint[2], data.joint[3], data.joint[4],
              data.joint[5])
    elif data.codeKey == MOVEP_CANFD_CB:  # 位姿透传
        print("透传结果:", data.errCode)
        print("当前角度:", data.joint[0], data.joint[1], data.joint[2], data.joint[3], data.joint[4],
              data.joint[5])
        print("当前位姿:", data.pose.position.x, data.pose.position.y, data.pose.position.z,
              data.pose.euler.rx, data.pose.euler.ry, data.pose.euler.rz)
    elif data.codeKey == FORCE_POSITION_MOVE_CB:  # 力位混合透传
        print("透传结果:", data.errCode)
        print("当前力度：", data.nforce)
    else:
        print("neither")



def doMsg(msg):
    global pose_list, pose_init
    pose01 = []
    [pose01.append(round(180 * msg.position[i] / pi,)) for i in range(0, len(msg.position))]
    robot.Movej_CANFD(pose01, False)
    # if pose_init != pose01:
    #      pose_list.append(pose01)
    #      rospy.loginfo(f"pose01: { pose01 }\n seq: { msg.header.seq }")
    #      pose_init = pose01
    # else:
    #      if pose_list != []:
    #         for i in range(0, len((pose_list))):
    #             robot.Movej_CANFD(pose_list[i], True, 0)
    #             print(pose_list[i])
    #         pose_list = [] 
#     rospy.loginfo(f"pose01: { pose01 }\n msg.position: { msg.position }\n type: { type(msg.position) }\n len: { len(msg.position) }\n num_type: { type(pose01[0]) }")


def topic_subscribe01():
       rospy.init_node("move_group_s", anonymous=True)
       sub = rospy.Subscriber("/joint_states", Jstate,doMsg,queue_size=50)
       rospy.spin()



def topic_subscribe02():
       rospy.init_node("move_group_s", anonymous=True)
       sub = rospy.Subscriber("/move_group/fake_controller_joint_states", Jstate,doMsg, queue_size=50)
       rospy.spin()


if __name__ == "__main__":
    global robot, pose_list, pose_init
    pose_init = [0, 0, 0, 0, 0, 0]
    pose_list = []
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    # robot.Movej_Cmd(pose_init, 20, 0)
    topic_subscribe01()
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()
